/*
 * Tracking_control.c
 *
 *  Created on: 2021年3月24日
 *      Author: Jiarun
 */
#include "tracking_control.h"


uint8 run_start = 1, finish = 0 , finish_sw = 1, row_cnt = 0, distan_flg, outlimit, debug_flag;
uint8 road_flag = 0, road_sw = 1, road_dir;
uint8 ramp_flag = 0;
uint16 ramp_distance = 0;
uint16 zebra;
uint32 ignore_time, road_time;
Param global_param;
Motor pid = {0};

int16 distance = 0;
//pid_param motor = {13,5,9,2000};

int16 constrain_int16(int16 amt, int16 low, int16 high)
{
    return ( amt < low ? low : (amt > high ? high : amt) );
}

int16 Bias_Motor;


int bangbang_e = 20;
void motor_PID()
{
    int16 error0,error1,error2;
    float P,I,D;
    int val = 13;//加减程度   数越大 加减越快 太大会引起骤停

    error0 = error1 = error2 = 0;
    error0 = pid.speed_set - pid.speed_real;  //I
    error1 = error0 - pid.lastError;            //P
    error2 = error0 - 2*pid.lastError + pid.preError;//D
    pid.preError = pid.lastError;
    pid.lastError = error0;

    if(outlimit)//停车标志
    {
        P=60; I=5; D=0;
    }
//    else if(ring_left_flag||ring_right_flag)//环岛
//    {
//        P=ring_speed_P; I=ring_speed_I; D=ring_speed_D;
//    }
    else
    {
        P=pid.P;    I=pid.I;      D=pid.D;
    }
    pid.PWM += (int16)(P*error1 + I*error0 + D*error2);
    /***************************************///PWM二次补偿  误差 倍数叠加


//        if(error0 > bangbang_e + 10 && pid.speed_set + 5 > global_param.speed_max[global_param.speed_level])
//        {                       //误差大  并且 速度大于速度公式的最大速度  满占空比加速
//
//        }
//        else
        {
            if(error0 >bangbang_e)
            {
                pid.PWM = pid.PWM + error0 * val;
            }
            else if(error0 < -bangbang_e)
            {
                pid.PWM = pid.PWM + error0 * val;
            }
        }

    pid.PWM = constrain_int16(pid.PWM, -Speed_Max, Speed_Max);
}

uint8 stopcar = 0;
extern uint8 outlimit;

/**
 * @brief  电机PWM信号输出
 * @param  speed    给定期望速度
 * @return none
 */
uint32 stop_timekeep;
void motor_ctrl(){
    static uint8 stop_time;

    motor_PID();

    /* 电机堵转时间大于0.5秒则置位 */
    if(stopcar && pid.speed_real > -15 && pid.speed_real < 15){

        if(!outlimit){
            stop_time ++;
        }

        if(stop_time > 60)
            stopcar |= 0x1;
        else{
            stop_timekeep = systick_getval_ms(STM0);
        }
    }else if(stop_time && systick_getval_ms(STM0) - stop_timekeep > 1200){
        stop_time = 0;
    }

    /* 电机堵转且有电磁信号 */
    if(stopcar & 0x1){
        pwm_duty(MOTOR_PIN, 0);
        pwm_duty(MOTOR_INVER, 0);
        pit_disable_interrupt(CCU6_1, PIT_CH0);
    }

#ifndef PWM_DISABLE

    else if(pid.PWM < 0)
    {
        pwm_duty(MOTOR_PIN, 0);
        pwm_duty(MOTOR_INVER, -pid.PWM);
//        buzzer_ON;
    }
    else{
        pwm_duty(MOTOR_PIN, pid.PWM);
        pwm_duty(MOTOR_INVER, 0);
    }

#endif

}

sint16 bias = 0, Last_bias = 0;
uint8 Kp;
void servo_ctrl(){
    uint8 count = 0, start_row, end_row, valid_row;
    int16 angle = 0;

    /* *************** 计算偏差 *************** */
//    if(road_flag && road_flag != 0xe0){
//        start_row = 20;
//        end_row = 40;
//    }
//    else
    {
        start_row = 25;
        end_row = 45;
    }
    for(uint8 row = start_row; row < end_row; row++){
        if(center[row] < 0)
            continue;

        bias += center[row];
        count ++;
    }

    if(count > 5){
        bias /= count;
        bias -= BIAS;
    }
    else
        bias = Last_bias;

/**************************************************/

    /* *************** 坡道 *************** */
    if(ramp_distance > 3000 && finish_sw && finish < 2){
        ramp_flag |= 0x1;
        road_flag = 0;
        buzzer_OFF;

        if(ramp_flag == 0x11){
            finish_sw = ramp_flag = 0;
            if(junction_cnt % 2){
                ++junction_cnt;
            }
            ignore_time = systick_getval_ms(STM0);
        }
    }
    else if(ramp_flag && ramp_distance < 300){
        ramp_flag = 0x10;
    }
/**************************************************/

    /* *************** 起跑线 *************** */
    if(zebra > 60 && junction_cnt > 0 && !ramp_flag && finish_sw){
        ++finish;
        bias = 0;
        if(junction_cnt % 2){
            ++junction_cnt;
        }
        ignore_time = systick_getval_ms(STM0);
        finish_sw = road_flag = 0;
        buzzer_OFF;
    }else if(zebra > 45 && !road_flag && valid_rowL > 10 && valid_rowR > 10){
        bias = constrain_int16(bias, -6, 6);
    }else if(road_sw && global_param.road_sw && !ramp_flag){
        /* 环岛检测 */
        roundabout();
    }
/**************************************************/

    /* *************** 停车 *************** */
    if(finish == 2){
        if(2 == global_param.run_dir){
            bias = 22;
        }else{
            bias = -22;
        }

        distan_flg = 1;
        if(distance > 5500){
            outlimit = 1;
        }
    }
    /* *************** 三岔 *************** */
    else if(junction){
        if(valid_rowL + valid_rowR < 39){
            distan_flg = 1;

            if(finish ^ global_param.Y_dir){
                bias = -20; //左
            }else{
                bias = 20;
            }
        }

        if(distance > 3000 && valid_rowL + valid_rowR > 33){
            junction = distance = distan_flg = 0;
            ++junction_cnt;
            road_flag = 0;
            buzzer_OFF;
        }
    }
    /* *************** 环岛 *************** */
    else if(road_flag){
        if(road_dir){
            if((road_flag & 0x1) || road_flag == 0xe0){
                if(road_flag & 0x4){
                    bias = -15;
                }else if(bias < -3){
                    bias += 5;
                }
            }else if(road_flag & 0x10){
                bias = -15;
            }
        }else{
            if((road_flag & 0x1) || road_flag == 0xe0){
                if(road_flag & 0x4){
                    bias = 15;
                }else if(bias > 3){
                    bias -= 5;
                }
            }else if(road_flag & 0x10){
                bias = 15;
            }
        }
    }
/**************************************************/

    /* *************** 计算舵机 P  *************** */
    valid_row = valid_rowL < valid_rowR ? valid_rowL : valid_rowR;

    if(crossroad_flag){
        /* 十字 */
        Kp = 12;
    }else if(pid.speed_set > global_param.speed_max[global_param.speed_level]){
        /* 加速适当减小 P */
        Kp = 16;
    }else{
        Kp = global_param.kp[global_param.speed_level] + (end_row - valid_row)*(end_row - valid_row) / global_param.advance;
    }
/**************************************************/


    /* *************** 发车 *************** */
    if(run_start){
        bias = 0;
        /*  */
        if((run_start & 0x2) || row_cnt < 9){
            if(2 == global_param.run_dir){
                bias = 30;
            }
            else if(1 == global_param.run_dir){
                bias = -30;
            }

            run_start |= 0x2;
            distan_flg = 1;
        }

        if(distance > 5000){
            distance = distan_flg = run_start = 0;
        }
    }

    angle = Kp * bias + global_param.kd[global_param.speed_level] * (bias-Last_bias);
    Last_bias = bias;

    pwm_duty(Servo_PIN, constrain_int16(Servo_Median + angle, Angle_min, Angle_max));
}

void roundabout(){
    /* 环岛 */
    if(!road_flag && !crossroad_flag && seek_finish_flag){
        /* 左 */
        if(valid_rowL < 10 && valid_rowR > 20 && brim_R_is_straight(20, 45, 18)){
            if(junction_cnt % 2 || zebra > 10){
                /* 三岔路内无环岛 */
            }else{
                road_dir = 1;
                road_flag |= 0x1;
                buzzer_ON;
            }
        }else if(valid_rowR < 10 && valid_rowL > 20 && brim_L_is_straight(20, 45, 18)){
            if(junction_cnt % 2 || zebra > 10){
                /* 三岔路内无环岛 */
            }else{
                road_dir = 0;
                road_flag |= 0x1;
                buzzer_ON;
            }
        }
    }

    /* 左 */
    if(road_dir){
        if((road_flag & 0x1) && valid_rowL > 15){
            road_flag |= 0x2;
        }
        if((road_flag & 0x2) && valid_rowL < 10){
            road_flag |= 0x4;
            distan_flg = 1;
        }
        if(road_flag & 0x4){
            if(distance > 3500 && valid_rowR > 22){
                road_flag = 0x8;
                distance = distan_flg = 0;
                buzzer_OFF;
            }
        }
        if(road_flag & 0x8){
            if(valid_rowR < 16 && zebra < 3){
                road_flag = 0x10;
                buzzer_ON;
            }
        }
        if(road_flag == 0x10 && valid_rowR > 20){
            road_flag = 0xe0;
            distan_flg = 1;
            buzzer_OFF;
        }
        if(road_flag == 0xe0 && distance > 2500 && valid_rowL > 19){
            road_flag = road_sw = distance = distan_flg = 0;
        }
    }else{
        if((road_flag & 0x1) && valid_rowR > 15){
            road_flag |= 0x2;
        }
        if((road_flag & 0x2) && valid_rowR < 10){
            road_flag |= 0x4;
            distan_flg = 1;
        }
        if(road_flag & 0x4){
            if(distance > 3500 && valid_rowL > 22){
                road_flag = 0x8;
                distance = distan_flg = 0;
                buzzer_OFF;
            }
        }
        if(road_flag & 0x8){
            if(valid_rowL < 16 && zebra < 3){
                road_flag = 0x10;
                buzzer_ON;
            }
        }
        if(road_flag == 0x10 && valid_rowL > 20){
            road_flag = 0xe0;
            distan_flg = 1;
            buzzer_OFF;
        }
        if(road_flag == 0xe0 && distance > 2500 && valid_rowR > 19){
            road_flag = road_sw = distance = distan_flg = 0;
            road_time = systick_getval_ms(STM0);
        }
    }
}
